/**
  *************************(C) COPYRIGHT 2022 LionHeart*************************
  * @file       recue_IDcard_task.c
  * @brief      救援卡片的兑换
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2022/4/5        yanfubing       1. done
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  *************************(C) COPYRIGHT 2022 LionHeart*************************
  */

#include "rescue_IDcard_task.h"
#include "cmsis_os.h"
#include "arm_math.h"
#include "bsp_rc.h"
#include "bsp_buzzer.h"
#include "detect_task.h"
#include "main.h"

rescue_IDcard_control_t rescue_IDcard_control;

/**
	* @brief          救援卡初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_Init(rescue_IDcard_control_t *rescue_idcard_control);

/**
	* @brief          救援卡状态
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_state_In(rescue_IDcard_control_t *rescue_idcard_control);

/**
	* @brief          救援卡任务模式设置
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_mode_set(rescue_IDcard_control_t *rescue_idcard_control);

/**
	* @brief          救援卡里程距
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void fall_down_mileage(rescue_IDcard_control_t* rescue_idcard_control);
/**
	* @brief          PID计算   
	* @retval         none
	*/
static void PID_realize(void);	

/**
	* @brief          限位   
	* @retval         none
	*/
static void Limit_location(void);

/**
	* @brief          救援卡任务
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
 void rescue_IDcard_task(void const* pvParameters)
 {
	 vTaskDelay(rescue_IDcard_task_INIT_TIME);
	 rescue_IDcard_Init(&rescue_IDcard_control);
	 vTaskDelay(rescue_IDcard_task_INIT_TIME);
	 while (toe_is_error(rescue_IDcard_motor_TOE))
	 {
		 vTaskDelay(rescue_IDcard_task_INIT_TIME);
	 }
    while (1)
    {
		Limit_location();
		fall_down_mileage(&rescue_IDcard_control);
		rescue_IDcard_mode_set(&rescue_IDcard_control);
		rescue_IDcard_state_In(&rescue_IDcard_control);
		PID_realize();
		vTaskDelay(5);
		// 电机发送电流信号的再fall_down_task.c中
    }
 }

/**
	* @brief           救援卡初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_Init(rescue_IDcard_control_t *rescue_idcard_control)
{
if (rescue_idcard_control == NULL)
	{
		return;
	}

		const static fp32 M2006_rescue_IDcard_mileage_pid[3] ={M2006_rescue_IDcard_MILEAGE_PID_KP,M2006_rescue_IDcard_MILEAGE_PID_KI,M2006_rescue_IDcard_MILEAGE_PID_KD};
		const static fp32 M2006_rescue_IDcard_speed_pid[3]   ={M2006_rescue_IDcard_SPEED_PID_KP,M2006_rescue_IDcard_SPEED_PID_KI,M2006_rescue_IDcard_SPEED_PID_KD};
		const static fp32 rescue_IDcard_order_filter[1]={engineering_Clamp_turn_NUM};

		rescue_idcard_control->rescue_IDcard_can_rc=get_can_control_point();

		rescue_idcard_control->rescue_IDcard_motor_data[0].rescue_IDcard_motor_measure=get_rescue_IDcard_point();

		PID_init(&rescue_idcard_control->rescue_IDcard_motor_mileage_pid[0], PID_POSITION, M2006_rescue_IDcard_mileage_pid, M2006_rescue_IDcard_MILEAGE_PID_MAX_OUT,
				M2006_rescue_IDcard_MILEAGE_PID_MAX_IOUT);
		PID_init(&rescue_idcard_control->rescue_IDcard_motor_pid[0], PID_POSITION, M2006_rescue_IDcard_speed_pid, M2006_rescue_IDcard_SPEED_PID_MAX_OUT,
				M2006_rescue_IDcard_SPEED_PID_MAX_IOUT);

		first_order_filter_init(&rescue_idcard_control->engineering_cmd_rescue_IDcard, rescue_IDcard_task_CONTROL_TIME, rescue_IDcard_order_filter);
		
		rescue_idcard_control->rescue_IDcard_task_max_speed=rescue_IDcard_task_MAX_SPEED;
		
		rescue_idcard_control->rescue_IDcard_state=CLOSE;
	
}
/**
	* @brief          pid计算
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
void PID_realize()
{

rescue_IDcard_control.rescue_IDcard_motor_data[0].speed_set
=PID_calc(&rescue_IDcard_control.rescue_IDcard_motor_mileage_pid[0], rescue_IDcard_control.rescue_IDcard_motor_data[0].angle,rescue_IDcard_control.rescue_IDcard_motor_data[0].mileage_set );

rescue_IDcard_control.rescue_IDcard_motor_data[0].give_current
= PID_calc(&rescue_IDcard_control.rescue_IDcard_motor_pid[0], rescue_IDcard_control.rescue_IDcard_motor_data[0].rescue_IDcard_motor_measure->speed_rpm,rescue_IDcard_control.rescue_IDcard_motor_data[0].speed_set );

}

/**
	* @brief         救援卡任务状态
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_state_In(rescue_IDcard_control_t *rescue_idcard_control)
{		
	if (rescue_idcard_control->rescue_IDcard_can_rc->key & KEY_PRESSED_OFFSET_C)
	{
     if ((!(rescue_idcard_control->rescue_IDcard_can_rc->key & KEY_PRESSED_OFFSET_CTRL))&&(!(rescue_idcard_control->rescue_IDcard_can_rc->key & KEY_PRESSED_OFFSET_SHIFT)))
		     {
		      rescue_idcard_control->rescue_IDcard_motor_data->mileage_set += CARD_ADD_MILEAGE;
		     }
	}
	else if (rescue_idcard_control->rescue_IDcard_can_rc->key & KEY_PRESSED_OFFSET_CTRL)
	{
if (!(rescue_idcard_control->rescue_IDcard_can_rc->key & KEY_PRESSED_OFFSET_C))
	{
    
	 	rescue_idcard_control->rescue_IDcard_motor_data->mileage_set -= 2*CARD_ADD_MILEAGE;
}
	}
	else return;

	  rescue_idcard_control->rescue_IDcard_motor_data->mileage_set=fp32_constrain(rescue_idcard_control->rescue_IDcard_motor_data->mileage_set,0, ID_Card_max_mileage);   //限幅函数
 
}

/**
	* @brief         救援卡任务模式设置
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void rescue_IDcard_mode_set(rescue_IDcard_control_t *rescue_idcard_control)
{
	if ((rescue_idcard_control->rescue_IDcard_can_rc->engineer_work_flg==0x00)||(rescue_idcard_control->rescue_IDcard_can_rc->engineer_work_flg==0x01))
	{
        rescue_idcard_control->rescue_IDcard_state =OPEN;
	}
	else
	{
		rescue_idcard_control->rescue_IDcard_state=CLOSE;  
	}		
}

/**
	* @brief          限位   
	* @retval         none
	*/
static void Limit_location(void)
{
  if(HAL_GPIO_ReadPin(P1_GPIO_Port,P1_Pin))
	{
	rescue_IDcard_control.rescue_IDcard_motor_data->speed_set=0;
	}
}

/**
	* @brief          救援卡里程距
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void fall_down_mileage(rescue_IDcard_control_t* rescue_idcard_control)
{
rescue_idcard_control->rescue_IDcard_motor_data[0].angle
=(rescue_idcard_control->rescue_IDcard_motor_data[0].rescue_IDcard_motor_measure->ecd_count*8191+rescue_idcard_control->rescue_IDcard_motor_data[0].rescue_IDcard_motor_measure->ecd)*MOTOR_ECD_TO_ANGLE;
}

/*******************************the end of file*************************************/

